#include <cmath>
#include <iostream>
#include <thread>

#include "asio_driver/device/buzzer_src.hpp"
#include "asio_driver/device/rc_sbus.hpp"
#include "asio_driver/io/timer.hpp"
#include "asio_driver/utils/range.hpp"
#include "asio_driver_special/imu_wit_cmp10a.hpp"
#include "asio_driver_special/motor_brt_can.hpp"
#include "asio_driver_special/motor_dc_pwm.hpp"
#include "ctrl_alg/filter/window_filter.hpp"
#include "ctrl_alg/pid/pid_pos.hpp"

static constexpr int rate = 20;

#include "chassis/recorder.hpp"

int main()
{
    Recorder recorder;
    recorder.SetPath("../../recorder.csv");

    std::cout << "init device!" << std::endl;
    AD::AsioContext::SharePtr context = AD::AsioContext::CreatAsioContext();
    std::unique_ptr<AD::Can> can0 = std::make_unique<AD::Can>(context);
    AD::CanBus::SharePtr can_bus_self = AD::CanBus::Create(context);

    std::unique_ptr<AD::Can> can1 = std::make_unique<AD::Can>(context);
    AD::CanBus::SharePtr can_bus_extern = AD::CanBus::Create(context);

    AD::Special::MotorBrtCan motorLeftAix1 = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorRightAix1 = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorLeftAix2 = AD::Special::MotorBrtCan(context);
    AD::Special::MotorBrtCan motorRightAix2 = AD::Special::MotorBrtCan(context);
    AD::Special::IMUWitCmp10a imu = AD::Special::IMUWitCmp10a(context);
    AD::RC_Sbus sbus = AD::RC_Sbus(context);
    AD::Buzzer buzzer = AD::Buzzer(context);

    motorLeftAix1.Open(can_bus_self, AD::Special::MotorBrtCan::MotorConfig{.address = 1, .poleCount = 14});
    motorRightAix1.Open(can_bus_self, AD::Special::MotorBrtCan::MotorConfig{.address = 2, .poleCount = 14});
    motorLeftAix2.Open(can_bus_self, AD::Special::MotorBrtCan::MotorConfig{.address = 4, .poleCount = 14});
    motorRightAix2.Open(can_bus_self, AD::Special::MotorBrtCan::MotorConfig{.address = 3, .poleCount = 14});

    can0->Open("can0");
    can1->Open("can1");
    can_bus_self->Open(std::move(can0));
    can_bus_extern->Open(std::move(can1));
    imu.Open("/dev/ttyS6", 115200);
    sbus.Open("/dev/ttyS7");
    buzzer.Open(34, true);

    CtrlAlg::PIDPos speedPID = CtrlAlg::PIDPos(100, 0, 0);
    // speedPID.SetMaxIntegralOutputPart(3);
    CtrlAlg::PIDPos gyroZPID = CtrlAlg::PIDPos(100, 0, 0);
    CtrlAlg::WindowFilter speedFilter = CtrlAlg::WindowFilter<double>(40);

    buzzer.Beep();
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    buzzer.StopBeep();
    std::cout << "start control!" << std::endl;

    //* pid self-blance
    recorder.CreatTitle({"time", "gyroZ", "speed", "targetSpeed", "targetGyroZ", "targetCurrent"});
    int speedTick = 0;

    float forwardVal = 0;
    float gyroZ = 0;

    while (context->Running())
    {
        // Get input
        auto rcFrame = sbus.ReadFrame();
        float speedCtrl = rcFrame.Channel(3, 0, 1);
        auto dirCtrl = rcFrame.ModeChannel(4);
        auto connect = rcFrame.ModeChannel(5);
        float forwardControl = rcFrame.Channel(1, -0.5, 0.5);
        float rotateControl = -rcFrame.Channel(0, -30, 30);
        // Read sensor
        auto imuData = imu.Read();
        gyroZ = AD::Utils::InRange(imuData.gyro[2], -60, 60) ? -imuData.gyro[2] : gyroZ;
        // motorLeftAix1.RequestRPM();
        // motorRightAix1.RequestRPM();
        // motorLeftAix2.RequestRPM();
        // motorRightAix2.RequestRPM();

        float speed = (motorLeftAix1.ReadState().rpm + motorRightAix1.ReadState().rpm + motorLeftAix2.ReadState().rpm +
                       motorRightAix2.ReadState().rpm) /
                      4 / 60 * 2 * 3.1415926 * 0.12;
        speed = speedFilter.GetControl(speed);
        recorder.SaveTime();
        recorder.SaveFloat(gyroZ);
        recorder.SaveFloat(speed);

        // if (std::abs(speed) > 1)
        //{
        //     std::cerr << "speed overlap: " << speed << std::endl;
        //     return 0;
        // }

        // Speed PID
        speedTick++;
        if (speedTick > 20)
        {
            speedTick = 0;
            forwardVal = speedPID.GetControl(forwardControl - speed, 1000.0 / rate);
        }
        forwardVal = forwardControl * 5000;

        // GyroZ PID
        // float gyroZVal = gyroZPID.GetControl(std::copysign(rotateControl, speedCtrl) - gyroZ, 1000.0 / rate);
        float gyroZVal = rotateControl / 30 * 5000;
        gyroZVal = AD::Utils::ClampAbs(gyroZVal, 5000);
        std::cout << "gyroZ: " << gyroZ << " gyroZVal: " << gyroZVal << std::endl;

        recorder.SaveFloat(forwardControl);
        recorder.SaveFloat(rotateControl);
        recorder.SaveFloat(forwardVal); // current

        // switch (dirCtrl)
        //{
        // case AD::RC_Sbus::Frame::ModeChannelValue::Up:
        //     forwardVal = speedCtrl * 5000;
        //     std::cout << "forward direction " << std::endl;
        //     break;
        // case AD::RC_Sbus::Frame::ModeChannelValue::Down:
        //     forwardVal = -speedCtrl * 5000;
        //     std::cout << "backward direction " << std::endl;
        //     break;
        // default:
        //     break;
        // }

        float left = forwardVal + gyroZVal;
        float right = forwardVal - gyroZVal;
        // left = AD::Utils::ClampAbs(left, 5000);
        // right = AD::Utils::ClampAbs(right, 5000);

        // float second =
        //     std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now().time_since_epoch())
        //         .count() /
        //     1000.0;
        // left = 500 * (1 + std::sin(second / 1));
        // right = left;
        // std::cout << "left: " << left << std::endl;

        //  Set Motor
        motorLeftAix1.SendHeartBeat();
        motorLeftAix1.SetCurrent(left);
        motorRightAix1.SendHeartBeat();
        motorRightAix1.SetCurrent(right);
        motorLeftAix2.SendHeartBeat();
        motorLeftAix2.SetCurrent(left);
        motorRightAix2.SendHeartBeat();
        motorRightAix2.SetCurrent(right);

        recorder.NewLine();
        context->SpinOnce();
        context->WaitRate(rate);
    }
    //*/
    recorder.Save();
    return 0;
}